#pragma once
#include "cseaamp.h"
#include "neuron.h"
#include "Matrix.h"

#define VELOCITY_CONTROL_ID 16
#define MAX_FORCE 0.03
#define DEFAULT_KP_VEL 0.03
#define DEFAULT_KD_VEL 0.00
#define DEFAULT_KI_VEL 0.12

#define LEARNING_RATE 0.12
#define HIDEN_DOT 3

#define ANGLE_ERROR 0

#define Q_DEFAULT 0.001
#define Rt_DEFAULT 0.02
#define Ra_DEFAULT ((double)50*PI*PI/180/180)
#define BASE 5


#define uint unsigned int 
using namespace std;

class CVelCrtl :
	public CAction
{
public:
	CVelCrtl(void);
	CVelCrtl( CSeaAmp &seaAmp ) { Init( seaAmp ); }
	~CVelCrtl(void);


	bool Init( CSeaAmp &seaAmp ) { return CAction::Init( seaAmp, VELOCITY_CONTROL_ID ); }

	void DoAct( void );

	void SetKP( double p ) { Kp = p; }
	double GetKP( void ) { return Kp; }

	void SetKI( double i ) { Ki = i; }
	double GetKI( void ) { return Ki; }

	void SetKD( double d ) { Kd = d; }
	double GetKD( void ) { return Kd; }

	double GetNeuronOutput() {return neuronOutput[0];}
	double GetForceOutput() {return output;}

	double GetPhase() {return compensatePhase;}

	char neuronUpdateFlag;
	char neuronFlag;
	neuron neuronNetwork;
	neuron neuronNetworkBasic;
	
private:
	double Kp;
	double Ki;
	double Kd;

	double delta[3];

	double output;

	ofstream VelocityData;

	//CQueryTimer timer;
	double timeInterval;


	
	double neuronOutput[OUTPUT_MAX];
	double *tempOutput;

	double neuronInput[INPUT_MAX];
	double limitInputPos[INPUT_MAX];
	double limitInputNeg[INPUT_MAX];
	double neuronDiff[INPUT_MAX];
	double impulseError[OUTPUT_MAX];
	double limitOutputPos[OUTPUT_MAX];
	double limitOutputNeg[OUTPUT_MAX];
	double neuronOutputError[OUTPUT_MAX];

	//State Variable Related 
	double compensateAmptitude;
	double compensatePhase;
	double ampError;
	double lastAmpError;
	double phaseError;
	double lastPhaseError;

	double learnRateA;
	double learnRateP;


	//EKF State Estimator Related
	void EKF_Update();
	void EKF_Init();
	void EKF_Record();
	CMatrix R;
	double Q;
	CMatrix K;
	CMatrix S; 
	CMatrix Jh;
	CMatrix tempMatrix;
	CMatrix Eye;

	ofstream EKFData;

	list<double> controlData;
	list<double> errorData;


	double Qestimator;
	double Restimator;

	double QRatio;
	double RRatio;

	void CovUpdate(void);

	//Particle State Estimator Related
	void Particle_Update();
	void Particle_Init();
	void Particle_Record();

	double particle[2][400];
	double weigth[400];

	double Possibility_Caculate(double error);



	
};

